/**
 * anis.darwich
 */

package clubhouse.motioncommand;

public class MotionCommand {
	
	private Integer start_delimiter;
	private Integer api_type;

	private Integer distance;
	private Integer rotation;
	private Integer trigo;
	
	private Integer end_delimiter;

	public MotionCommand (Integer type, Integer a, Integer b, Integer c) {
		this.start_delimiter = 255;
		
		this.api_type = type; // 16 ou 17
		
		this.distance = a;
		this.rotation = b;
		this.trigo = c;
		
		this.end_delimiter = 254;
	}
	
	public MotionCommand () {
		this.start_delimiter = 255;
		this.end_delimiter = 254;
	}
	
	public Integer getApiType() {
		return this.api_type;
	}
	
	public Integer getDistancePulse() {
		return this.distance;
	}
	
	public Integer getRotationPulse() {
		return this.rotation;
	}
	
	public Integer getRotationDirection() {
		return this.trigo;
	}
	
	public double[] getCoordIncrement() {
		if (this.trigo == 0) this.rotation= -rotation;
		return new double[] {
			(distance/50)*Math.cos(rotation*Math.PI/100),
			(distance/50)*Math.sin(rotation*Math.PI/100),
			rotation*Math.PI/100};
	}
	
	public String toHexPrint(){
		return (Integer.toHexString(start_delimiter)+":" + Integer.toHexString(api_type)+":" + Integer.toHexString(distance)+":" + Integer.toHexString(rotation)+":" + Integer.toHexString(trigo)+":" + Integer.toHexString(end_delimiter));
	}
	
	public byte[] toSend() {
		byte[] send_buffer = new byte[7];
		send_buffer[0] = this.start_delimiter.byteValue();
		send_buffer[1] = this.api_type.byteValue();
		send_buffer[2] = (byte)( (this.distance << 16) >> 24 );
		send_buffer[3] = (byte)( (this.distance << 24) >> 24 );
		send_buffer[4] = this.rotation.byteValue();
		send_buffer[5] = this.trigo.byteValue();
		send_buffer[6] = this.end_delimiter.byteValue();
		return send_buffer;
	}
	
	public static MotionCommand parse(String packet) {
		MotionCommand received = new MotionCommand();
		received.api_type = Integer.parseInt(packet.substring(2, 4), 16);
		received.distance = Integer.parseInt(packet.substring(4, 8), 16);
		received.rotation = Integer. parseInt(packet.substring(8, 10), 16);
		received.trigo = Integer. parseInt(packet.substring(10, 12), 16);
		
		/*
		System.out.print("length " + packet.length()+ "\n"); //
		System.out.print("@ index 0-2: " + packet.substring(0, 2) + "\n"); // ff start delimiter
		System.out.print("@ index 2-4: " + packet.substring(2, 4) + "\n"); // 10 ou 11 api type
		System.out.print("@ index 4-8: " + packet.substring(4, 8) + "\n"); // 10 distance
		System.out.print("@ index 8-10: " + packet.substring(8, 10) + "\n"); // 3 rotation
		System.out.print("@ index 10-12: " + packet.substring(10, 12) + "\n"); // 0 ou 1 rotation direction
		*/
		
		return received;
	}
}	
	